#!/usr/bin/env python
# -*- coding: UTF-8 -*-
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
import rospy

if __name__ == '__main__':
    rospy.init_node('endpub', anonymous=True)
    # tell the action client that we want to spin a thread by default
    ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    # wait for the action server to come up
    while not ac.wait_for_server(rospy.Duration(5.0)):
        rospy.loginfo("Waiting for the move_base action server to come up")

    goal = MoveBaseGoal()
    # we'll send a goal to the robot to move 1 meter forward
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()

    goal.target_pose.pose = Pose(
        Point(-0.2, -5.3, 0.06), Quaternion(0, 0, -1, 0))

    rospy.loginfo("Sending goal")
    ac.send_goal(goal)
    ac.wait_for_result()
    if ac.get_state() == actionlib.simple_action_client.GoalStatus.SUCCEEDED:
        rospy.loginfo("success")
    else:
        rospy.loginfo(
            "failed")
